home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 46 / Amiga Format CD46 (1999-10-20)(Future Publishing)(GB)[!][issue 1999-12].iso / -in_the_mag- / reader_requests / scilab / demos / wheel2 / maple / wheel.map < prev    next >
Text File  |  1999-09-16  |  10KB  |  308 lines
  1. read(`Euler.map`);
  2. #----------------------------------------------------------------------------
  3. # AND NOW THE WHEEL
  4. #-----------------------------------------------------------------------------
  5.  
  6. #----------------------------------------------------------------------------
  7. # TeX Notations for my problem 
  8. #-----------------------------------------------------------------------------
  9.  
  10. ll:= {x = `x`  , y = `y` , z= `z`,theta = `\\theta`, phi = `\\phi`,psi = `\\psi`,param =`\\alpha`, t = `\\eta` , LL = `\\LL` } :
  11. addnotations(ll):
  12.  
  13. #----------------------------------------------------------------------------
  14. # Lagrangian for The wheel problem : 
  15. #---------------------------------------------------------------------------
  16.  
  17. # Number of parameters:
  18. mpar:=5:
  19. param:=[m,J[1],J[2],r,g]:
  20. # Number of state variables
  21. n:=6:
  22.  
  23. # Lagragian as a function of q[k], qd[k] k=1,..,n and
  24. # param[m] m=1,..,mpar
  25. # Lagrangian variables :
  26. q := [ x,y,z,theta,phi,psi]:
  27. qd := [ seq ( cat(q[i],`d`),i=1..n)]:
  28. qdd:= [ seq ( cat(q[i],`dd`),i=1..n)]:
  29.  
  30. #-------------------------------------------
  31. # Geometric computations 
  32. #-------------------------------------------
  33. Bx:=vector([1,0,0]):
  34. By:=vector([0,1,0]):
  35. Bz:=vector([0,0,1]):
  36. Bu1:=vector([cos(theta),sin(theta),0]):
  37. Bu:= add(Bu1,Bz,cos(phi),-sin(phi)):
  38. Bw:= add(Bu1,Bz,sin(phi),cos(phi)):
  39. Bv:=crossprod(Bw,Bu):
  40. Bv:=map((i)->factor(combine(i,trig)),Bv):
  41.  
  42. R:=transpose(matrix(3,3,[eval(Bu),eval(Bv),eval(Bw)])):
  43. Rinv:=map((i)->factor(combine(i,trig)),inverse(R)):
  44.  
  45. # Omega in the (x,y,z) base 
  46.  
  47. OmegaX:= add(Bz,add(Bv,Bw,phid,psid),thetad,1):
  48.  
  49. # Omega in the (u,v,w) base
  50.  
  51. Omega:=multiply(Rinv,OmegaX):
  52. Omega:=map((i)->factor(combine(i,trig)),Omega):
  53.  
  54. # J[1]=J[2]=J_u : J[3]=J_w
  55.  
  56. EcJ:= J[1]*(Omega[1]^2+Omega[2]^2) +J[3]*Omega[3]^2:
  57.  
  58. L:=(1/2)*( param[1]*(qd[1]^2+qd[2]^2+qd[3]^2) + EcJ )
  59.           -param[1]*param[5]*q[3]:
  60.  
  61. sorties(`systeme.tex`,`Lagrangian`,L):
  62.  
  63. # Lhs of Euler equations 
  64.  
  65. EL:=euler_equations(L,q,qd,qdd):
  66. E:=map((i)->rhs(i),EL):
  67.  
  68. # don't forget the LL macro in all.tex 
  69.  
  70. sorties(`systeme.tex`,`variables :`,` q ` = matrix(nops(q),1,q)):
  71. sortiesM(`systeme.tex`,`Lhs of Euler Equations`,[seq(EL[i,1],i=1..nops(q))]):
  72.  
  73. #-----------------------------------------------------
  74. # Rewritting the Euler equations to have a canonical form 
  75. #           ..         .
  76. # El= ME(q)  q + CE(q) q^2 + RE(q,...)
  77. # Computation of ME,CE,RE 
  78. #-----------------------------------------------------
  79.  
  80. XX:=CEuler(E,q,qd,qdd):
  81.  
  82. # simplifying notations for output
  83.  
  84. sortiesI(`systeme.tex`,`$M(q)\\ddot{q}+C(q)\\dot{q}^2 +R(q,\\dot{q})$`):
  85. sorties(`systeme.tex`,`M~:`,XX[1]):
  86. sorties(`systeme.tex`,`C~:`,XX[2]):
  87. sorties(`systeme.tex`,`R~:`,XX[3]):
  88.  
  89. #------------------------------------------------
  90. # Constraints on the Wheel
  91. #------------------------------------------------
  92.  
  93. # geometric constraints
  94.  
  95. hc:=[q[3]-param[4]*sin(q[5])]:
  96.  
  97. # dynamic constraints ( contains the derivative of geometric constraint )
  98. # v_g + crossprod(OmegaX,Bu)=0
  99.  
  100. gg:=add(vector([qd[1],qd[2],qd[3]]),crossprod(OmegaX,Bu),1,r):
  101. gg:=map((i)->simplify(expand(i)),gg):
  102.  
  103. nhc:=convert(gg,list):
  104.  
  105. sortiesM(`systeme.tex`,`Geometric Constraints hc=0,hc:`,hc);
  106. sortiesM(`systeme.tex`,`Dynamic Constraints nhc=0,nhc`,nhc);
  107.  
  108. # Derivatives of constraints are of type Aprim qd = 0 
  109.  
  110. Aprim:=genmatrix(nhc,qd):
  111.  
  112. sorties(`systeme.tex`,`Constraints : $A'(q)\\dot{q}$`,Aprim);
  113.  
  114. #----------------------------------------------
  115. # Computing SS:=S(q);
  116. # S(q) will solve Aprim S(q) = 0 
  117. # in The Euler equations  Equ= A(q)' lambda + u 
  118. # the term A(q)lambda can be eliminated 
  119. # if we left-multiply euler equations by S(q)'
  120. #----------------------------------------------
  121. ncont:=3;
  122. SS:=linsolve(Aprim,matrix(ncont,1,0)):
  123.  
  124. sorties(`systeme.tex`,` S(q)`,SS);
  125.  
  126. #------------------------------------------------------------------------
  127. # The constraints are now dotq=S(q)eta 
  128. # can be used to see that eta =[phi,theta,psi]
  129. # ( eta[i] is the maple variable ti )
  130. # but the indices can be mixed and linsolve doesn't 
  131. # always return the same result 
  132. # We have to check the correspondance between t.sig(i)=[phi,theta,psi]
  133. # and to change SS to have a good corespondance 
  134. #-------------------------------------------------------------------------
  135. knsize:=3;
  136. permut:={seq(SS[i+3,1]=t_s[i],i=1..knsize)}:
  137. SS:=subs(permut,eval(SS)):
  138. permut:={seq(t_s[i]=t[i],i=1..knsize)}:
  139. SS:=subs(permut,eval(SS)):
  140.  
  141. S:= genmatrix(convert(convert(SS,vector),list),[seq( t[i],i=1..knsize)]):
  142. sorties(`systeme.tex`,`$\\dot{q}=S(q)\\eta$ Kernel of $A(q)'$~:`,S):
  143.  
  144. #-----------------------------------------------------
  145. # this multiplication eliminates the term A(q) lambda 
  146. # in the Euler equations 
  147. #-----------------------------------------------------
  148.  
  149. E1:=multiply(transpose(S),E):
  150.  
  151. # sortiesM(`systeme.tex`,`$S(q)^T E$`,E1);
  152.  
  153. #-----------------------------------------------------
  154. # since Aprim(q) dotq=0 
  155. # .
  156. # q= S(q) eta ; here  eta = [t1,t2]
  157. #                                 ..
  158. # we use this equation to compute q
  159. # Warning : t1 and t2 are time dependent
  160. #-----------------------------------------------------
  161.  
  162. qt  := [ seq (t[i]  ,i=1..knsize)]:
  163. qtd := [ seq (td[i] ,i=1..knsize)]:
  164. qtdd:= [ seq (tdd[i],i=1..knsize)]:
  165.  
  166. qqdd:=map((x,y,z,t)-> time_diff(x,y,z,t),eval(SS),
  167.     [op(q),op(qt)],[op(qd),op(qtd)],[op(qdd),op(qtdd)]):
  168.  
  169. #-----------------------------------------------------
  170. #       ..                       .
  171. # using  q= d/dt [ S(q) eta] and q= S(q) eta 
  172. # we can subsitute these expressions in E1
  173. #-----------------------------------------------------
  174.  
  175. E2:=subs(seq(qdd[i]=qqdd[i,1],i=1..nops(qdd)),eval(E1)):
  176. E3:=subs(seq(qd[i]=SS[i,1],i=1..nops(qd)),eval(E2)):
  177.  
  178. #-----------------------------------------------------
  179. # The global system is now 
  180. #             .
  181. #  E3 = 0 and q= S(q) eta 
  182. #-----------------------------------------------------
  183.  
  184. E3:=map((x)-> simplify(x),E3):
  185.  
  186. sortiesM(`systeme.tex`,
  187.     `$S(q)^T E$ simplified with $\\dot{q}=S(q)\\eta $`,E3);
  188.  
  189. #------------------------------------------------------------------
  190. # Trying to find canonical representation 
  191. # for the simplified euler equations
  192. #            .
  193. # El= ME(q)  t + RE(q,t)
  194. # we use CEuler with a little trick in the parameter call qt,qt,qtd
  195. #------------------------------------------------------------------
  196.  
  197. XX1:=CEulerP(E3,qt,qt,qtd):
  198.  
  199. MM3:=map((i)->factor(combine(i,trig)),XX1[1]):
  200. RR3:=map((i)->factor(combine(i,trig)),XX1[2]):
  201.  
  202. sortiesI(`systeme.tex`,`a cononical form $M(q)\\dot{t}+R(q,t)$`);
  203. sorties(`systeme.tex`,`C:`,MM3);
  204. sorties(`systeme.tex`,`R:`,RR3);
  205.  
  206. #-----------------------------------------------------
  207. # FORTRAN GENERATION 
  208. #-----------------------------------------------------
  209.  
  210. #-----------------------------------------------------
  211. # First routine wheel(neq,t,z,zdot)
  212. # z= [ A,dotA, X] ou A=[theta,phi,psi] X=x,y
  213. #       |0  I  0  |
  214. # zdot =|0  0  0  | + Y
  215. #       |0 S1(q) 0|   ( S1 : 2-first rows of S)
  216. # where Y solves M(q)Y + C(q)dotA^2 + R =0 
  217. #-----------------------------------------------------
  218. kn:=knsize:
  219.  
  220. fvar:= {theta= z[1],phi=z[2],psi=z[3],t[1]=z[4],
  221.     t[2]=z[5],t[3]=z[6],x=z[7],y=z[8]}:
  222. MM3F:=subs(fvar,eval(MM3)):
  223.  
  224.  # don't forget the minus sign 
  225. RR3F:=map((x)-> -x ,subs(fvar,eval(RR3))):
  226. SSF:=subs(fvar,eval(SS)):
  227.  
  228. flist:=[subroutinem,`wheel`,[`neq`,`t`,`z`,`zdot`],
  229.            [
  230.             [ declaref,`implicit double precision`,[`(t)`] ],
  231.             [ parameterf,[`kn=`.kn]],
  232.             [ declaref,doubleprecision,[`t,z(8),zdot(8),r,j(3),m`]],
  233.             [ declaref,doubleprecision,[`me3s(kn,kn)`]],
  234.             [ declaref,doubleprecision,[`const(kn,1),w(3*kn),rcond`]],
  235.             [ declaref,integer,[`i,k,neq,ierr`]],
  236.             [ declaref,`data g`,[`/ 9.81/`]],
  237.             [ declaref,`data r`,[`/ 1.0/`] ],
  238.             [ declaref,`data m`,[`/ 1.0/`] ],
  239.             [ declaref,`data j`,[`/ 0.3,0.4,1.0/`] ],
  240.             [ matrixm,`me3s`,MM3F ] ,
  241.             [ matrixm,`const`,RR3F ] , 
  242.         [ dom , `i ` ,1,`kn `,1,[ equalf,`zdot(i)`,`z(i+kn)`]],
  243.         [commentf,` we must solve  M z =const `],
  244.         [ callf , `dlslv`,[`me3s,kn,kn,Const,kn,1,w, rcond,ierr,1`]],
  245.         [ if_then_m,ierr<>0,[
  246.         [writef,6,ff_w,[]],
  247.         [formatf,ff_w,[`'Matrice mal conditionnee'`]]]],
  248.         [ dom , `i ` ,1,`kn `,1,[ equalf,`zdot(kn+i)`,`const(i,1)`]],
  249.         [ equalf, zdot(7),SSF[1,1]],
  250.         [ equalf, zdot(8),SSF[2,1]],
  251.         [returnf]]]:
  252.  
  253.  
  254. Gener(`wheel.f`,flist):
  255.  
  256. #-----------------------------------------------------
  257. # second routine wheelg(n,k,uf,vf,wf,xx)
  258. # n,k integer 
  259. # uf,vf,wf ==> matrices of size (n,k)
  260. # xx solution of ode => matrix of size(8,k)
  261. # This routines will computes the coordinates of trajectories of n-points 
  262. # in the (x,y,z) space , givent their trajectories in the (u,v,w) space 
  263. # xx(:,t) gives is the evolution of the wheel 
  264. # uf,vf,wf : on entry the coordinates in the (u,v,w) space 
  265. # uf,vf,wf : on output the coordinates in the (x,y,z) space 
  266. #-----------------------------------------------------
  267.  
  268. ffvar:= {theta= xx[1,i2],phi=xx[2,i2],psi=xx[3,i2],t[1]=xx[4,i2],
  269.     t[2]=xx[5,i2],t[3]=xx[6,i2],x=xx[7,i2],y=xx[8,i2],
  270.  
  271.     uf = uf[i1,i2],vf = vf[i1,i2] ,wf= wf[i1,i2] }:
  272.  
  273.  
  274. Z:=multiply(R,vector([uf,vf,wf])):
  275. Z:=add(vector([x,y,r*sin(phi)]),Z,1,r):
  276. ZF:=subs(ffvar,eval(Z)):
  277.  
  278. fffvar:={ cos(xx[1,i2])=cs1,cos(xx[2,i2])=cs2,sin(xx[1,i2])=si1,sin(xx[2,i2])=si2}:
  279. ZF:=subs(fffvar,eval(ZF)):
  280.  
  281. flist:=[subroutinem,`wheelg`,[`n,k,uf,vf,wf,xx`],
  282.            [
  283.             [ declaref,`implicit double precision`,[`(t)`] ],
  284.             [ declaref,doubleprecision,[`uf(n,k),vf(n,k),wf(n,k)`]],
  285.             [ declaref,doubleprecision,[`uu,vv,ww,r`]],
  286.             [ declaref,integer,[`n,k,i1,i2`]],
  287.             [ declaref,doubleprecision,[`xx(8,k)`]],
  288.             [ declaref,`data r`,[`/ 1.0/`] ],
  289.         [ dom , `i1 ` ,1,`n `,1,[
  290.         [ dom , `i2 ` ,1,`k `,1,
  291.             [
  292.              op(map((x)-> [ equalf,rhs(x),lhs(x) ],fffvar)),
  293.              [ equalf,`uu`,ZF[1]],
  294.              [ equalf,`vv`,ZF[2]],
  295.              [ equalf,`ww`,ZF[3]],
  296.              [ equalf,`uf(i1,i2)`,`uu`],
  297.              [ equalf,`vf(i1,i2)`,`vv`],
  298.              [ equalf,`wf(i1,i2)`,`ww`]]]]],
  299.         [returnf]]]:
  300.  
  301.  
  302. Gener(`wheelg.f`,flist):
  303.  
  304.  
  305.  
  306.  
  307.  
  308.